/***************************************************************************
*   Copyright (C) 2010-2011 by swkyer <swkyer@gmail.com>                  *
*                                                                         *
*   This program is free software; you can redistribute it and/or modify  *
*   it under the terms of the GNU General Public License as published by  *
*   the Free Software Foundation; either version 2 of the License, or     *
*   (at your option) any later version.                                   *
*                                                                         *
*   This program is distributed in the hope that it will be useful,       *
*   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
*   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
*   GNU General Public License for more details.                          *
*                                                                         *
*   You should have received a copy of the GNU General Public License     *
*   along with this program; if not, write to the                         *
*   Free Software Foundation, Inc.,                                       *
*   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
***************************************************************************/
#include "stdafx.h"
#include <stdio.h>
#include <stdlib.h>
#include "Jlink.h"


jlink_t *jlink_open(jtagdev_cfg_t *pcfg)
{
    int retv;
    uint32_t mask;
    jlink_t *pjlink;

    pjlink = (jlink_t *)malloc(sizeof(jlink_t));
    if (pjlink == NULL)
        return NULL;

    pjlink->cfg = *pcfg;
    pjlink->usbdev = jlinkusb_open(pjlink->cfg.devnum);
    if (pjlink->usbdev == NULL)
    {
        free(pjlink);
        return NULL;
    }

    pjlink->state = TAP_INVALID;

    pjlink->jacc.numbits = 0;
    pjlink->jacc.tdi = (uint8_t *)malloc(EJTAG_MAX_BIT_STREAM_LENGTH/sizeof(uint8_t));
    if (pjlink->jacc.tdi == NULL)
    {
        free(pjlink);
        return NULL;
    }
    pjlink->jacc.tms = (uint8_t *)malloc(EJTAG_MAX_BIT_STREAM_LENGTH/sizeof(uint8_t));
    if (pjlink->jacc.tms == NULL)
    {
        free(pjlink->jacc.tdi);
        free(pjlink);
        return NULL;
    }
    pjlink->jacc.tdo = (uint8_t *)malloc(EJTAG_MAX_BIT_STREAM_LENGTH/sizeof(uint8_t));
    if (pjlink->jacc.tdo == NULL)
    {
        free(pjlink->jacc.tms);
        free(pjlink->jacc.tdi);
        free(pjlink);
        return NULL;
    }

    retv = jlinkusb_get_caps(pjlink->usbdev, &pjlink->caps);
    if (retv != 0)
    {
        fprintf(stderr, "faild to read caps...\n");
        jlink_close(pjlink);
        return NULL;
    }
    if (pjlink->caps.caps & EMU_CAP_GET_CPU_CAPS)
    {
        retv = jlinkusb_get_caps_ex(pjlink->usbdev, &pjlink->caps);
        if (retv != 0)
        {
            fprintf(stderr, "faild to read caps ex...\n");
            jlink_close(pjlink);
            return NULL;
        }
    }
    if (pjlink->caps.caps & EMU_CAP_SPEED_INFO)
    {
        retv = jlinkusb_get_speed(pjlink->usbdev, &pjlink->speed);
        if (retv != 0)
        {
            fprintf(stderr, "faild to get speed...\n");
            jlink_close(pjlink);
            return NULL;
        }
    }
    if (pjlink->caps.caps & EMU_CAP_EX_GET_HW_INFO)
    {
        retv = jlinkusb_get_hw_info(pjlink->usbdev, &pjlink->hwinfo);
        if (retv != 0)
        {
            fprintf(stderr, "faild to get hardware info...\n");
            jlink_close(pjlink);
            return NULL;
        }
    }
    retv = jlinkusb_get_state(pjlink->usbdev, &pjlink->pinstate);
    if (retv != 0)
    {
        fprintf(stderr, "faild to get pin status...\n");
        jlink_close(pjlink);
        return NULL;
    }
    retv = jlinkusb_get_unknow_09(pjlink->usbdev, &pjlink->u09, 0);
    if (retv != 0)
    {
        fprintf(stderr, "faild to get unknow 09 CMD...\n");
        jlink_close(pjlink);
        return NULL;
    }
    retv = jlinkusb_get_unknow_09(pjlink->usbdev, &pjlink->u09, 1);
    if (retv != 0)
    {
        fprintf(stderr, "faild to get unknow 09 CMD...\n");
        jlink_close(pjlink);
        return NULL;
    }

    retv = jlinkusb_select_if(pjlink->usbdev, 0xFF, &mask);
    if (retv != 0)
    {
        fprintf(stderr, "faild to get interface...\n");
        jlink_close(pjlink);
        return NULL;
    }
    if (mask & TIF_JTAG)
    {
        retv = jlinkusb_select_if(pjlink->usbdev, 0x00, &mask);
        if (retv != 0)
        {
            fprintf(stderr, "faild to set interface 0x00...\n");
            jlink_close(pjlink);
            return NULL;
        }
    }
    else
    {
        fprintf(stderr, "DO NOT support JTAG...\n");
        jlink_close(pjlink);
        return NULL;
    }

    return pjlink;
}

int jlink_close(jlink_t *pjlink)
{
    if (pjlink->usbdev != NULL)
        jlinkusb_close(pjlink->usbdev);

    free(pjlink->jacc.tms);
    free(pjlink->jacc.tdi);
    free(pjlink->jacc.tdo);
    free(pjlink);

    return 0;
}

jlink_t *jlink_reinit(jlink_t *pjlink)
{
    jtagdev_cfg_t cfg;

    if (pjlink == NULL)
        return NULL;
    
    cfg = pjlink->cfg;

    jlink_close(pjlink);
    return jlink_open(&cfg);
}

int jlink_is_target_connect(jlink_t *pjlink)
{
    int retv;

    if (pjlink == NULL)
        return -1;

    retv = jlinkusb_get_state(pjlink->usbdev, &pjlink->pinstate);
    if (retv != 0)
    {
        fprintf(stderr, "faild to get pin status...\n");
        return -2;
    }

    // FIXME
    if (pjlink->pinstate.voltage <= 1000)
        return 0;
    return 1;
}

int jlink_trst_activate(jlink_t *pjlink)
{
    int retv;

    if (pjlink == NULL)
        return -1;

    retv = jlinkusb_hw_trst0(pjlink->usbdev);
    if (retv != 0)
        return retv;
    pjlink->state = TAP_INVALID;
    return 0;
}

int jlink_trst_deactivate(jlink_t *pjlink)
{
    int retv;

    if (pjlink == NULL)
        return -1;

    retv = jlinkusb_hw_trst1(pjlink->usbdev);
    if (retv != 0)
        return retv;
    pjlink->state = TAP_RESET;
    return 0;
}

int jlink_rst_activate(jlink_t *pjlink)
{
    if (pjlink == NULL)
        return -1;

    return jlinkusb_hw_reset0(pjlink->usbdev);
}

int jlink_rst_deactivate(jlink_t *pjlink)
{
    if (pjlink == NULL)
        return -1;

    return jlinkusb_hw_reset1(pjlink->usbdev);
}

static const uint8_t bitvals[8] = {0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80};
static const uint8_t bitvals_rev[8] = {0xfe, 0xfd, 0xfb, 0xf7, 0xef, 0xdf, 0xbf, 0x7f};
static void jlink_jacc_build(jlink_jtag_acc_t *pacc, int tdi, int tms)
{
    int curbyt = pacc->numbits / sizeof(uint8_t);
    int curbit = pacc->numbits % sizeof(uint8_t);
    if (tms)
        pacc->tms[curbyt] |= bitvals[curbit];
    else
        pacc->tms[curbyt] &= bitvals_rev[curbit];
    if (tdi)
        pacc->tdi[curbyt] |= bitvals[curbit];
    else
        pacc->tdi[curbyt] &= bitvals_rev[curbit];
    pacc->numbits++;
}

static int jlink_jacc_debuild(jlink_jtag_acc_t *pacc)
{
    int curbyt = pacc->numbits / sizeof(uint8_t);
    int curbit = pacc->numbits % sizeof(uint8_t);
    pacc->numbits++;
    return (pacc->tdo[curbyt] & bitvals[curbit]) ? 1 : 0;
}

int jlink_tap_reset(jlink_t *pjlink, int use_trst)
{
    int retv;

    if (pjlink == NULL)
        return -1;

    if (use_trst)
    {
        jlink_trst_activate(pjlink);
        Sleep(pjlink->cfg.trst_time);
        jlink_trst_deactivate(pjlink);

        pjlink->state = TAP_RESET;
        return 0;
    }

    // TMS=1, 5 times from anywhere to TAP_RESET
    pjlink->jacc.numbits = 0;
    jlink_jacc_build(&pjlink->jacc, 1, 1);
    jlink_jacc_build(&pjlink->jacc, 1, 1);
    jlink_jacc_build(&pjlink->jacc, 1, 1);
    jlink_jacc_build(&pjlink->jacc, 1, 1);
    jlink_jacc_build(&pjlink->jacc, 1, 1);

    retv = jlinkusb_hw_jtag3(pjlink->usbdev, &pjlink->jacc);
    if (retv != 0)
        return retv;

    pjlink->state = TAP_RESET;
    return 0;
}

int jlink_runtest_idle(jlink_t *pjlink)
{
    int retv;

    if (pjlink == NULL)
        return -1;

    // build the tms/tdi bit stream, from TAP_RESET to TAP_IDLE
    pjlink->jacc.numbits = 0;
    if (pjlink->state != TAP_RESET)
    {
        jlink_jacc_build(&pjlink->jacc, 1, 1);
        jlink_jacc_build(&pjlink->jacc, 1, 1);
        jlink_jacc_build(&pjlink->jacc, 1, 1);
        jlink_jacc_build(&pjlink->jacc, 1, 1);
        jlink_jacc_build(&pjlink->jacc, 1, 1);
    }
    jlink_jacc_build(&pjlink->jacc, 1, 0);

    retv = jlinkusb_hw_jtag3(pjlink->usbdev, &pjlink->jacc);
    if (retv != 0)
        return retv;

    pjlink->state = TAP_IDLE;
    return 0;
}

tap_state_t jlink_get_tap_state(jlink_t *pjlink)
{
    if (pjlink == NULL)
        return TAP_INVALID;
    return pjlink->state;
}

int jlink_tap_moveto(jlink_t *pjlink, tap_state_t dest)
{
    if (pjlink == NULL)
        return -1;

    // TODO
    return -1;
}

int jlink_instruction(jlink_t *pjlink, ubase_t ir_out, ubase_t *pirin)
{
    int i, retv;
    ubase_t irin = 0;

    if (pjlink == NULL)
        return -1;

    if (pjlink->state != TAP_IDLE)
        jlink_runtest_idle(pjlink);

    // build the tms/tdi bit stream
    pjlink->jacc.numbits = 0;
    jlink_jacc_build(&pjlink->jacc, 1, 1);				// TAP_DRSELECT
    jlink_jacc_build(&pjlink->jacc, 1, 1);				// TAP_IRSELECT
    jlink_jacc_build(&pjlink->jacc, 1, 0);				// TAP_IRCAPTURE
    jlink_jacc_build(&pjlink->jacc, 1, 0);				// TAP_IRSHIFT
    for (i=0; i<pjlink->cfg.ir_len-1; i++)
    {
        if (ir_out & 0x01)
            jlink_jacc_build(&pjlink->jacc, 1, 0);
        else
            jlink_jacc_build(&pjlink->jacc, 0, 0);
        ir_out >>= 1;
    }
    if (ir_out & 0x01)
        jlink_jacc_build(&pjlink->jacc, 1, 1);			// TAP_IREXIT1
    else
        jlink_jacc_build(&pjlink->jacc, 0, 1);
    jlink_jacc_build(&pjlink->jacc, 1, 1);				// TAP_IRUPDATE
    jlink_jacc_build(&pjlink->jacc, 1, 0);				// TAP_IDLE

    retv = jlinkusb_hw_jtag3(pjlink->usbdev, &pjlink->jacc);
    if (retv != 0)
        return retv;

    // get the return value
    if (pirin)
    {
        pjlink->jacc.numbits = 0;
        jlink_jacc_debuild(&pjlink->jacc);				// TAP_DRSELECT
        jlink_jacc_debuild(&pjlink->jacc);				// TAP_IRSELECT
        jlink_jacc_debuild(&pjlink->jacc);				// TAP_IRCAPTURE
        jlink_jacc_debuild(&pjlink->jacc);				// TAP_IRSHIFT
        for (i=0; i<pjlink->cfg.ir_len; i++)
        {
            if (jlink_jacc_debuild(&pjlink->jacc))
                irin |= (1<<i);
        }
        *pirin = irin;
    }

    pjlink->state = TAP_IDLE;
    return 0;
}

int jlink_data(jlink_t *pjlink, ubase_t data_out, ubase_t *pdatain)
{
    int i, retv;
    ubase_t datain = 0;

    if (pjlink == NULL)
        return -1;

    if (pjlink->state != TAP_IDLE)
        jlink_runtest_idle(pjlink);

    // build the tms/tdi bit stream
    pjlink->jacc.numbits = 0;
    jlink_jacc_build(&pjlink->jacc, 1, 1);				// TAP_DRSELECT
    jlink_jacc_build(&pjlink->jacc, 1, 0);				// TAP_DRCAPTURE
    jlink_jacc_build(&pjlink->jacc, 1, 0);				// TAP_DRSHIFT
    for (i=0; i<pjlink->cfg.dr_len-1; i++)
    {
        if (data_out & 0x01)
            jlink_jacc_build(&pjlink->jacc, 1, 0);
        else
            jlink_jacc_build(&pjlink->jacc, 0, 0);
        data_out >>= 1;
    }
    if (data_out & 0x01)
        jlink_jacc_build(&pjlink->jacc, 1, 1);			// TAP_DREXIT1
    else
        jlink_jacc_build(&pjlink->jacc, 0, 1);
    jlink_jacc_build(&pjlink->jacc, 1, 1);				// TAP_DRUPDATE
    jlink_jacc_build(&pjlink->jacc, 1, 0);				// TAP_IDLE

    retv = jlinkusb_hw_jtag3(pjlink->usbdev, &pjlink->jacc);
    if (retv != 0)
        return retv;

    // get the return value
    if (pdatain)
    {
        pjlink->jacc.numbits = 0;
        jlink_jacc_debuild(&pjlink->jacc);				// TAP_DRSELECT
        jlink_jacc_debuild(&pjlink->jacc);				// TAP_DRCAPTURE
        jlink_jacc_debuild(&pjlink->jacc);				// TAP_DRSHIFT
        for (i=0; i<pjlink->cfg.dr_len; i++)
        {
            if (jlink_jacc_debuild(&pjlink->jacc))
                datain |= (1<<i);
        }
        *pdatain = datain;
    }

    pjlink->state = TAP_IDLE;
    return 0;
}
